#ifndef ENIGMA_H
#define ENIGMA_H

#include <cmath>
#include "Affector.h"
#include "Entity.h"
#include "Field.h"
#include "Physics.h"
#include "Singularity.h"
#include "SpringRoot.h"
#include "SpringAxis.h"
#include "Vec2d.h"
#include "Vec3d.h"
#include "Vector.h"

// the equimass functions save a divide and a couple multiplies; 
// spherical treats the entities as billiard balls, while axial treats them as boxes.
// The dimensions of the entities are only used to depenetrate them (keep them from sticking), not for any collision detection or as a collision parameter
void collideSpherical(Entity& e1, float radius1, Entity& e2, float radius2);
void collideSphericalEquimass(Entity& e1, float radius1, Entity& e2, float radius2);
void collideAxial(Entity& e1, float w1, float h1, Entity& e2, float w2, float h2);
void collideAxialEquimass(Entity& e1, float w1, float h1, Entity& e2, float w2, float h2);

void springForce(Entity& e1, Entity& e2, float k, float L);

void collideSpherical(Entity& e1, float radius1, Entity& e2, float radius2)
{
	Vec2d relativeDir = (e2.pos - e1.pos).normalizePrecise();
	Vec2d impulse = relativeDir * ((e1.vel - e2.vel) * relativeDir);
			
	/*
	 //depenetrate which only moves the object that was moving the fastest (seems to work best)
	 if (e1.vel * e1.vel > e2.vel * e2.vel)
	 e1.pos = e2.pos - relativeDir * (radius1 + radius2);
	 else
	 e2.pos = e1.pos + relativeDir * (radius1 + radius2);
	 */
	
	/*
	 //depenetrate which moves both objects and takes into account their individual radii
	 float radiiRatio = radius1 / (radius1 + radius2);
	 Vec2d midpoint = e1.pos * (1 - radiiRatio) + e2.pos * (radiiRatio);
	 e1.pos = midpoint + relativeDir * -radius1;
	 e2.pos = midpoint + relativeDir * radius2;
	 */
	
	if (impulse * relativeDir > 0)//if spheres are moving toward each other (not away from each other)
	{
		float impulseFactor = 2.f / (e1.getMass() + e2.getMass());		
	
		e1.vel = e1.vel - impulse * (e2.getMass() * impulseFactor);
		e2.vel = e2.vel + impulse * (e1.getMass() * impulseFactor);
	}
}

void collideSphericalEquimass(Entity& e1, float radius1, Entity& e2, float radius2)
{
	//get unit vector pointing from e1 to e2
	Vec2d relativeDir = (e2.pos - e1.pos).normalizePrecise();
	
	//note that e1.vel - e2.vel gives velocity of e1 in the frame of e2
	//and note use of dot product here (max impact when dir and velocity difference align; min when they are at right angles (a graze))
	Vec2d impulse = relativeDir * ((e1.vel - e2.vel) * relativeDir);
	
	/*
	//depenetrate
	if (e1.vel * e1.vel > e2.vel * e2.vel)
		e1.pos = e2.pos - relativeDir * (radius1 + radius2);
	else
		e2.pos = e1.pos + relativeDir * (radius1 + radius2);
	*/
	 
	//apply the collision impulse
	if (impulse * relativeDir > 0)//if spheres are moving toward each other (not away from each other)
	{
		e1.vel = e1.vel - impulse;
		e2.vel = e2.vel + impulse;
	}
}

void collideAxial(Entity& e1, float w1, float h1, Entity& e2, float w2, float h2)
{
	// For a given direction, using the center of mass frame to find a solution that conserves momentum and kinetic energy gives the formula:
	// 2 * Velocity_CM - Velocity_Old = Velocity_New, where Vcm = (m0 * v0 + m1 * v1) / (m0 + m1)
	Vec2d disp = e2.pos - e1.pos;
	float midpoint; //will only be in one dimension
	float VcmX2 = 2.f / (e1.getMass() + e2.getMass());//first factor of twice the velocity in center of mass frame (it is independent of the dimension of the collision)
	
	//if the centers align more horizontally, the collision occurred along the x axis; if more vertical, along the y
	if (fabs(disp.x) > fabs(disp.y))
	{
		midpoint = (e1.pos.x + e2.pos.x) * 0.5f;
		//depenetrate
		if (e1.pos.x < e2.pos.x)
		{
			e1.pos.x = midpoint - w1 * 0.5f;
			e2.pos.x = midpoint + w2 * 0.5f;
		}
		else
		{
			e1.pos.x = midpoint + w1 * 0.5f;
			e2.pos.x = midpoint - w2 * 0.5f;
		}
		VcmX2 = VcmX2 * (e1.getMass() * e1.vel.x + e2.getMass() * e2.vel.x);
		e1.vel.x = VcmX2 - e1.vel.x;
		e2.vel.x = VcmX2 - e2.vel.x;
	}
	else
	{
		midpoint = (e1.pos.y + e2.pos.y) * 0.5f;
		if (e1.pos.y < e2.pos.y)
		{
			e1.pos.y = midpoint - h1 * 0.5f;
			e2.pos.y = midpoint + h2 * 0.5f;
		}
		else
		{
			e1.pos.y = midpoint + h1 * 0.5f;
			e2.pos.y = midpoint - h2 * 0.5f;
		}
		VcmX2 = VcmX2 * (e1.getMass() * e1.vel.y + e2.getMass() * e2.vel.y);
		e1.vel.y = VcmX2 - e1.vel.y;
		e2.vel.y = VcmX2 - e2.vel.y;
	}
}

void collideAxialEquimass(Entity& e1, float w1, float h1, Entity& e2, float w2, float h2)
{
	Vec2d disp = e2.pos - e1.pos;
	float temp; //will be midpoint first
	
	//if the centers align more horizontally, the collision occurred along the x axis; if more vertical, along the y
	if (fabs(disp.x) > fabs(disp.y))
	{
		//depenetrate
		temp = (e1.pos.x + e2.pos.x) * 0.5f;
		if (e1.pos.x < e2.pos.x)
		{
			e1.pos.x = temp - w1 * 0.5f;
			e2.pos.x = temp + w2 * 0.5f;
		}
		else
		{
			e1.pos.x = temp + w1 * 0.5f;
			e2.pos.x = temp - w2 * 0.5f;
		}
		//swap velocities in the dimension of the collision
		temp = e1.vel.x; //now used as temp variable
		e1.vel.x = e2.vel.x;
		e2.vel.x = temp;
	}
	else
	{
		temp = (e1.pos.y + e2.pos.y) * 0.5f;
		if (e1.pos.y < e2.pos.y)
		{
			e1.pos.y = temp - h1 * 0.5f;
			e2.pos.y = temp + h2 * 0.5f;
		}
		else
		{
			e1.pos.y = temp + h1 * 0.5f;
			e2.pos.y = temp - h2 * 0.5f;
		}
		temp = e1.vel.y;
		e1.vel.y = e2.vel.y;
		e2.vel.y = temp;
	}
}

void springForce(Entity& e1, Entity& e2, float k, float L)
{
	//k is the spring constant, where F = -k * x
	//L is the equilibrium length of the spring
	Vec2d disp = e2.pos - e1.pos;
	float force = (-k) * (sqrt(disp * disp) - L);
	Vec2d dir = disp.normalize(); //direction points from e1 to e2; a negative force pulls the entities toward each other
	e2.accel = e2.accel + dir * force * e2.getInvMass();
	e1.accel = e1.accel - dir * force * e1.getInvMass();
}

#endif



